#pragma once
#ifndef COLLLISION_DETECTOR_H
#define COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include "CollisionDetector2D.h"

typedef Kernel_d::Vector_d Vector_d;

class CollisionDetector {
public:
	
	CollisionDetector(vector<Polygon_2>* robot_polygons, Obstacles* obs, bool combinedMeasure, double clearanceAlpha) 
		: m_combinedMeasure(combinedMeasure), m_clearanceAlpha(clearanceAlpha)
	
	{
		m_robot1_maze_col = new CollisionDetector2D(robot_polygons->at(0), obs);
		m_robot2_maze_col = new CollisionDetector2D(robot_polygons->at(1), obs);

		m_robot1_as_obstacles.push_back(robot_polygons->at(0));
		m_robot2_robot1_col = new CollisionDetector2D(robot_polygons->at(1), &m_robot1_as_obstacles);
		
	}
	
	~CollisionDetector()
	{
		delete m_robot1_maze_col;
		delete m_robot2_maze_col;
		delete m_robot2_robot1_col;
	};

	////////////////////////
	// Queries
	////////////////////////
	
	/*	Check if the given configuration is collision free (returns true for free samples) */
	bool	valid_conf(Point_d pos)
	{
		Point_2 robot1 = Point_2(pos[0], pos[1]);
		Point_2 robot2 = Point_2(pos[2], pos[3]);
		Vector_2 robotDiff = robot2 - robot1;
		Point_2 diffPoint = Point_2(robotDiff.x(), robotDiff.y());

		bool robot1Valid = m_robot1_maze_col->valid_conf(robot1);
		bool robot2Valid = m_robot2_maze_col->valid_conf(robot2);
		bool robotDiffValid = m_robot2_robot1_col->valid_conf(diffPoint); 
		return (robot1Valid && robot2Valid && robotDiffValid);
	}

	

	/*	Validate the connection between two configurations by 
		sampling along a line.
		Input:	* Start and target configurations
				* Eps value that determines the size of the steps 
					that are taken along the line
	*/
	bool	local_planner(Point_d start, Point_d target, double eps) const {
		Point_2 robot1Start = Point_2(start[0], start[1]);
		Point_2 robot2Start = Point_2(start[2], start[3]);
		Point_2 robot1Target = Point_2(target[0], target[1]);
		Point_2 robot2Target = Point_2(target[2], target[3]);

		Vector_2 distanceStartVector = (robot2Start - robot1Start);
		Point_2 distanceStart = Point_2(distanceStartVector.x(), distanceStartVector.y());
		Vector_2 distanceEndVector = (robot2Target - robot1Target);
		Point_2 distanceEnd = Point_2(distanceEndVector.x(), distanceEndVector.y());

		bool robot1Free = m_robot1_maze_col->local_planner(robot1Start, robot1Target, eps);
		bool robot2Free = m_robot2_maze_col->local_planner(robot2Start, robot2Target, eps);
		bool interRobotFree = m_robot2_robot1_col->local_planner(distanceStart, distanceEnd, eps);

		return robot1Free && robot2Free && interRobotFree;
	}

	double cost_of_path(Point_d start, Point_d target) const {
		Point_2 robot1Start = Point_2(start[0], start[1]);
		Point_2 robot2Start = Point_2(start[2], start[3]);
		Point_2 robot1Target = Point_2(target[0], target[1]);
		Point_2 robot2Target = Point_2(target[2], target[3]);

		Vector_2 distanceStartVector = (robot2Start - robot1Start);
		Point_2 distanceStart = Point_2(distanceStartVector.x(), distanceStartVector.y());
		Vector_2 distanceEndVector = (robot2Target - robot1Target);
		Point_2 distanceEnd = Point_2(distanceEndVector.x(), distanceEndVector.y());

		double robot1Distance = sqrt(CGAL::to_double(CGAL::squared_distance(robot1Start, robot1Target)));
		double robot2Distance = sqrt(CGAL::to_double(CGAL::squared_distance(robot2Start, robot2Target)));
		if (m_combinedMeasure) {
			double robot1Clearance= m_robot1_maze_col->clearance_along_path(robot1Start, robot1Target);
			double robot2Clearance = m_robot2_maze_col->clearance_along_path(robot2Start, robot2Target);
			double interRobotClearance = m_robot2_robot1_col->clearance_along_path(distanceStart, distanceEnd);

			double robot1Score = std::min(robot1Clearance, interRobotClearance);
			double robot2Score = std::min(robot2Clearance, interRobotClearance);

			robot1Distance /= std::pow(robot1Score, m_clearanceAlpha);
			robot2Distance /= std::pow(robot2Score, m_clearanceAlpha);
		}

		return robot1Distance + robot2Distance;
		
	}

	////////////////////////
	// Data Members
	////////////////////////
	CollisionDetector2D* m_robot1_maze_col;
	CollisionDetector2D* m_robot2_maze_col;
	CollisionDetector2D* m_robot2_robot1_col;
	Obstacles m_robot1_as_obstacles;
	bool m_combinedMeasure;
	double m_clearanceAlpha;
};

#endif
